function [latLonAlt] = ecef2lla_cg( p, f, RE, nIter) %#codegen
%ECEF2LLA_CG 将地球系坐标转换成经纬高，适用于代码生成版本
%
% Tips:
% # 地球坐标系选择GwEN
%
% Input Arguments:
% # p: m*3矩阵，每行3个元素分别对应地球系坐标x、y、z，单位为m
% # f: 标量，地球模型椭球体的扁率，无单位
% # RE: 标量，地球模型椭球体的半长轴，单位m
% # nIter: 标量，迭代次数，无单位，默认为5
%
% Output Arguments:
% # latLonAlt: m*3矩阵，每行3个元素分别对应纬度、经度、高度，单位分别为rad、rad、m
%
% Assumptions and Limitations:
% # 本函数不适用于地球系坐标z为零的情况
%
% References:
% #《应用导航算法工程基础》“地球系坐标与经纬高的转换”

if nargin < 4
    nIter = 5;
end

m = size(p, 1);
lat = NaN(m, 1);
latLonAlt = coder.nullcopy(NaN(m, 3));
eSq = 1 - (1-f)^2;
latLonAlt(:, 2) = atan2(p(:, 2), p(:, 1));

t = p(:, 3) ./ sqrt(p(:, 2).^2 + p(:, 1).^2);
latLonAlt(:, 1) = atan(t ./ (1-f)^2);

count = 0;
RNPlush = NaN(m, 1);
RN = NaN(m, 1);
while any(lat ~= latLonAlt(:, 1)) && (count<=nIter)
    lat = latLonAlt(:, 1);
    RN = RE./sqrt(cos(latLonAlt(:, 1)).^2+(1-eSq)*sin(latLonAlt(:, 1)).^2);
    RNPlush = p(:, 1)./cos(latLonAlt(:, 1))./cos(latLonAlt(:, 2));
    latLonAlt(:, 1) = atan(RNPlush .* t ./ (RNPlush-RN.*eSq));	% Eq. 7.46
    count = count + 1;
end
latLonAlt(:, 3) = RNPlush-RN;
end